import io
import json
import pickle
import sys
import time
import grpc
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import patches

from btpg.envs.RoboWaiter.proto import camera
from btpg.envs.RoboWaiter.proto import semantic_map
from btpg.envs.RoboWaiter.algos.navigator.navigate import Navigator
import math
from btpg.envs.RoboWaiter.proto import GrabSim_pb2
from btpg.envs.RoboWaiter.proto import GrabSim_pb2_grpc
import copy
import os

# from robowaiter.scene.ui import scene_ui
from btpg.envs.RoboWaiter.utils.basic import get_root_path
from sklearn.cluster import DBSCAN
from matplotlib import pyplot as plt
from btpg.envs.RoboWaiter.algos.navigator.dstar_lite import euclidean_distance
from PIL import Image
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas

plt.rcParams['font.sans-serif'] = ['SimHei']  # 用来正常显示中文标签
plt.rcParams['axes.unicode_minus'] = False  # 用来正常显示负号

root_path = get_root_path()

channel = grpc.insecure_channel(
    "localhost:30001",
    options=[
        ("grpc.max_send_message_length", 1024 * 1024 * 1024),
        ("grpc.max_receive_message_length", 1024 * 1024 * 1024),
    ],
)
animation_step = [4, 5, 7, 3, 3]
loc_offset = [-700, -1400]

objname_en2zh_dic_goal = {
'Customer':'顾客',
'Banana': '香蕉',
'Toothpaste': '牙膏',
'Bread': '面包',
'Softdrink': '盒装饮料',
'Yogurt': '酸奶',
'ADMilk': '安佳牛奶',
'VacuumCup': '真空杯',
'Bernachon': '贝尔纳松',
'BottledDrink': '瓶装饮料',
'PencilVase': '铅笔花瓶',
'Teacup': '茶杯',
'Dictionary': '字典',
'Cake': '蛋糕',
'Stapler': '订书机',
'LunchBox': '午餐盒',
'Bracelet': '手镯',
'CoconutWater': '椰水',
'Walnut': '核桃',
'HamSausage': '火腿肠',
'GlueStick': '胶棒',
'AdhesiveTape': '胶带',
'Calculator': '计算器',
'Chess': '棋',
'Orange': '橙子',
'Glass': '玻璃杯',
'Washbowl': '洗碗盆',
'Durian': '榴莲',
'Gum': '口香糖',
'Towel': '毛巾',
'OrangeJuice': '橙汁',
'Cardcase': '卡包',
'RubikCube': '魔方',
'StickyNotes': '便签',
'NFCJuice': 'NFC果汁',
'SpringWater': '矿泉水',
'Apple': '苹果',
'Coffee': '咖啡',
'Gauze': '纱布',
'Mangosteen': '山竹',
'SesameSeedCake': '芝麻糕',
'Glove': '手套',
'Mouse': '鼠标',
'Kettle': '水壶',
'Atomize': '喷雾器',
'Chips': '薯片',
'SpongeGourd': '丝瓜',
'Garlic': '大蒜',
'Potato': '土豆',
'Tray': '托盘',
'Hemomanometer': '血压计',
'TennisBall': '网球',
'ToyDog': '玩具狗',
'ToyBear': '玩具熊',
'TeaTray': '茶盘',
'Sock': '袜子',
'Scarf': '围巾',
'ToiletPaper': '卫生纸',
'Milk': '牛奶',
'Novel': '小说',
'Watermelon': '西瓜',
'Tomato': '番茄',
'CleansingFoam': '洁面泡沫',
'CoconutMilk': '椰奶',
'SugarlessGum': '无糖口香糖',
'MedicalAdhesiveTape': '医用胶带',
'PaperCup': '纸杯',
'Caddy': '茶匙',
'Date': '日期',
'MilkDrink': '牛奶饮品',
'CocountWater': '椰水',
'AdhensiveTape': '胶带',
'Towl': '毛巾',
'Soap': '肥皂',
'CocountMilk': '椰奶',
'MedicalAdhensiveTape': '医用胶带',
'SourMilkDrink': '酸奶饮品',
'Tissue': '纸巾',
'YogurtDrink': '酸奶饮品',
'Newspaper': '报纸',
'Box': '盒子',
'PaperCupStarbucks': '星巴克纸杯',
'CoffeeMachine': '咖啡机',
'GingerLHand': '机器人左手',
'GingerRHand': '机器人右手',
'Straw': '吸管',
'Door': '门',
'Machine': '机器',
'PackagedCoffee': '包装咖啡',
'CubeSugar': '方糖',
'Spoon': '勺子',
'Drinks': '饮料',
'Drink': '饮料',
'Take-AwayCup': '外带杯',
'Saucer': '茶杯',
'TrashBin': '垃圾桶',
'Knife': '刀',
'Ginger': '姜',
'Floor': '地板',
'Roof': '屋顶',
'Wall': '墙',
'Broom':'扫帚',
'CoffeeCup': '咖啡杯',
'Mug': '马克杯',
'ZhuZi': '著子',
'LaJiTong': '垃圾桶',
'ZaoTai': '灶台',
'Sofa': '沙发',
'ChaTou': '插头',
'Plate': '盘子',
'CoffeeBag': '咖啡袋',
'TuoBu': '拖布',
'KaiGuan': '开关',
'ChaZuo': '插座',
'Sugar': '糖',
'BaTai': '吧台',
'BaoJing': '报警',
'DrinkMachine': '饮料机',
'KongTiao': '空调',
'Desk': '桌子',
'Clip': '夹子',
'TuoPan': '托盘',
'BoJi': '簸箕',
'ZhiBeiHe': '纸杯盒',
'WaterCup': '水杯',
'Chair': '椅子',
'Hand': '手',
'XiGuan': '吸管',
'Container': '容器',
'IceMachine': '制冰机',
'KaoXiang': '烤箱',
'SaoBa': '扫把',
'XiangGui': '香柜',
}
objname_zh2en_dic_goal = dict(zip(objname_en2zh_dic_goal.values(), objname_en2zh_dic_goal.keys()))
# print(objname_zh2eh_dic)

def show_image(camera_data):
    print('------------------show_image----------------------')
    # 获取第0张照片
    im = camera_data.images[0]
    # 使用numpy(np) 数值类型矩阵的frombuffer，将im.data以流的形式（向量的形式）读入，在变型reshape成三位矩阵的形式(长度，宽度，深度)即三阶张量
    d = np.frombuffer(im.data, dtype=im.dtype).reshape((im.height, im.width, im.channels))
    # matplotlib中的plt方法 对矩阵d 进行图形绘制，如果 深度相机拍摄的带深度的图片（图片名字中有depth信息），则转换成黑白图即灰度图
    plt.imshow(d, cmap="gray" if "depth" in im.name.lower() else None)
    # 图像展示在屏幕上
    # plt.show()

    return d


class Scene:
    robot = None
    event_list = []
    new_event_list = []
    signal_event_list = []
    # show_bubble = True
    event_signal = "None"
    # step_interval = 1
    # camera_interval = 1.5
    output_path = os.path.join(os.path.dirname(__file__), "outputs")

    objname_en2zh_dic = objname_en2zh_dic_goal
    objname_zh2en_dic = objname_zh2en_dic_goal

    default_state = {
        "map": {
            "2d": None,
            "obj_pos": {}
        },
        "chat_list": [],  # 未处理的顾客的对话, (顾客的位置,顾客对话的内容)
        "sub_goal_list": [],  # 子目标列表
        "status": None,  # 仿真器中的观测信息，见下方详细解释
        # "condition_set": {'At(Robot,Bar)', 'Is(AC,Off)',
        #                   'Exist(Yogurt)', 'Exist(BottledDrink)','Exist(Softdrink)',
        #                   # 'On(Yogurt,Bar)','On(BottledDrink,Bar)',
        #                   # 'Exist(Softdrink)', 'On(Softdrink,Table1)',
        #                   'Exist(Chips)', 'Exist(NFCJuice)', 'Exist(Bernachon)', 'Exist(ADMilk)', 'Exist(SpringWater)',
        #                   'Holding(Nothing)',
        #                   # 'Holding(Yogurt)',
        #                   'Exist(VacuumCup)', 'On(VacuumCup,Table2)',
        #                   'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
        #                   'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'},

    "condition_set": {'RobotNear(Bar)',
                        'Not Active(AC)','Not Active(HallLight)','Active(TubeLight)','Not Closed(Curtain)',
                        'Not Exists(Coffee)','Not Exists(Water)','Not Exists(Dessert)',
                        'Holding(Nothing)',
                        'Not IsClean(Table1)', 'Not IsClean(Floor)', 'Not IsClean(Chairs)',
                        'On(Softdrink,Table1)','On(VacuumCup,Table2)',

            # 'On(Yogurt,Bar)','On(BottledDrink,Bar)','On(ADMilk,Bar)','On(Chips,Bar)',
            # 'On(Softdrink,Table1)', 'On(Softdrink,Table3)',
            # 'On(ADMilk,Bar)','On(Bernachon,Bar)','On(SpringWater,Bar2)','On(MilkDrink,Bar)',
            # 'On(VacuumCup,Table2)',
            },


        # "condition_set": {'At(Robot,Bar)', 'Is(AC,Off)',
        #           'Exist(Yogurt)','Exist(VacuumCup)',
        #           'Holding(Nothing)',
        #           'On(Yogurt,Bar)','On(VacuumCup,Table2)',
        #           'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
        #           'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'},

        # "condition_set": {'At(Robot,Bar)', 'Is(AC,Off)',
        #                   'Exist(VacuumCup)','Exist(Coffee)',
        #                   'On(VacuumCup,Table2)', 'On(Coffee,CoffeeTable)',
        #                   'Holding(Nothing)',
        #                   'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
        #                   'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'},

        # "condition_set": {'At(Robot,Bar)', 'Is(AC,Off)',
        #                   'Exist(Yogurt)', 'Exist(VacuumCup)', 'Exist(Coffee)',
        #                   'Holding(Nothing)',
        #                   'On(Yogurt,Bar)', 'On(VacuumCup,Table2)', 'On(Coffee,CoffeeTable)',
        #                    'Is(HallLight,Off)', 'Is(TubeLight,On)',
        #                   'Is(Curtain,On)',
        #                   'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'},
    "obj_mem": {},
        "customer_mem": {},
        "served_mem": {},
        "greeted_customers": set(),
        "attention": {},
        "serve_state": {},
        "chat_history": {},
        "wait_history": set(),
        "anomaly": None
    }
    """
    status:
        location: Dict[X: float, Y: float]
        rotation: Dict[Yaw: float]
        joints: List[Dict[name: str, location: Dict[X: float, Y: float, Z: float]]]
        fingers: List[Dict[name: str, location: List[3 * Dict[X: float, Y: float, Z: float]]]]
        objects[:-1]: List[Dict[name: str, location: Dict[X: float, Y: float, Z: float]]]
        objects[-1]: Dict[name: "Hand", boxes: List[Dict[diagonals: List[4 * Dict[X0: float, Y0: float, Z0: float, X1: float, Y1: float, Z1: float]]]]]
        walkers: List[name: str, pose: Dict[X: float, Y: float, Yaw: float], speed: float, target: Dict[X: float, Y: float, Yaw: float]]
        timestamp: int, timestep: int
        collision: str, info: str
    """

    def __init__(self, robot=None, sceneID=0):
        self.stub = GrabSim_pb2_grpc.GrabSimStub(channel)

        self.robot = robot

        self.sceneID = sceneID
        self.use_offset = False
        self.start_time = time.time()
        self.time = 0
        self.sub_task_seq = None
        os.makedirs(self.output_path,exist_ok=True)

        self.show_bubble = True
        # 是否展示UI
        self.show_ui = False
        # 图像分割
        self.take_picture = False
        self.map_ratio = 5
        self.map_map = np.zeros((math.ceil(950 / self.map_ratio), math.ceil(1850 / self.map_ratio)))
        self.db = DBSCAN(eps=self.map_ratio, min_samples=int(self.map_ratio / 2))
        self.infoCount = 0

        self.is_nav_walk = False

        self.bt_algo_opt = True

        file_name = os.path.join(root_path,'robowaiter/algos/navigator/map_5.pkl')
        if os.path.exists(file_name):
            with open(file_name, 'rb') as file:
                self.map_map_real = pickle.load(file)



        self.robot_changed = False
        self.last_event_time = 0
        self.last_camera_time = -99999
        self.last_step_time = -99999

        self.img_cache = {
            "img_label_canvas":None,
            "img_label_seg":None,
            "img_label_obj":None,
            "img_label_map":None,
        }

        # 1-7 正常执行, 8-10 控灯操作移动到6, 11-12窗帘操作不需要移动,
        # self.op_dialog = ["", "制作咖啡", "倒水", "夹点心", "拖地", "擦桌子", "开筒灯", "搬椅子",  # 1-7
        #                   "关筒灯", "开大厅灯", "关大厅灯", "关闭窗帘", "打开窗帘",  # 8-12
        #                   "调整空调开关", "调高空调温度", "调低空调温度",  # 13-15
        #                   "抓握物体", "放置物体"]  # 16-17
        self.op_dialog = [
            "",  # 空字符串，可能用作默认值或占位符
            "Making Coffee",  # 制作咖啡
            "Pouring Water",  # 倒水
            "Grabbing Snacks",  # 抓取零食
            "Mopping Floor",  # 拖地
            "Wiping Table",  # 擦桌子
            "Turning on Tube Light",  # 打开灯管灯
            "Moving Chairs",  # 移动椅子
            "Turning off Tube Light",  # 关闭灯管灯
            "Turning on Hall Light",  # 打开大厅灯
            "Turning off Hall Light",  # 关闭大厅灯
            "Closing Curtains",  # 关闭窗帘
            "Opening Curtains",  # 打开窗帘
            "Toggling Air Conditioning",  # 切换空调状态
            "Increasing AC Temperature",  # 提高空调温度
            "Decreasing AC Temperature",  # 降低空调温度
            "Grasping Object",  # 抓取物体
            "Placing Object"  # 放置物体
        ]

        # 动画控制的执行步骤数
        self.op_act_num = [0, 3, 4, 6, 3, 2, 0, 1,
                           0, 0, 0, 0, 0,
                           0, 0, 0,
                           0, 0]
        # 动画控制的执行区域坐标
        self.op_v_list = [[0.0, 0.0], [250.0, 310.0], [-70.0, 480.0], [250.0, 630.0], [-70.0, 740.0], [260.0, 1120.0],
                          [300.0, -220.0],
                          [0.0, -70.0]]
        self.op_typeToAct = {8: [6, 2], 9: [6, 3], 10: [6, 4], 11: [8, 1], 12: [8, 2]}  # 任务类型到行动的映射
        self.obj_loc = [300.5, -140.0, 114]  # 空调面板位置

        # AEM
        self.visited = set()
        self.all_frontier_list = set()
        self.semantic_map = semantic_map
        self.auto_map = np.ones((800, 1550))
        self.filename = os.path.join(root_path, 'robowaiter/proto/map_1.pkl')
        with open(self.filename, 'rb') as file:
            self.map_file = pickle.load(file)

        self.colors = [
            'red',
            'pink',
            'purple',
            'blue',
            'cyan',
            'green',
            'yellow',
            'orange',
            'brown',
            'gold',
        ]

        # tool register
        self.all_loc_en = ['bar', 'Table', 'sofa', 'stove', 'Gate', 'light switch', 'airconditioner switch', 'cabinet',
                           'bathroom', 'window', 'audio',
                           'lounge area', 'workstation', 'service counter', 'cashier counter', 'corner', 'cake display',
                           'ChargingStations',
                           'refrigerator', 'bookshelf']

        self.loc_map_en = {'bar': {'工作台', '服务台', '收银台', '蛋糕柜'}, 'Table': {'大门', '休闲区', '墙角'},
                           'sofa': {'餐桌', '窗户', '音响', '休闲区', '墙角', '书架'},
                           'stove': {'吧台', '橱柜', '工作台', '服务台', '收银台', '蛋糕柜', '冰箱'},
                           'Gate': {'吧台', '灯开关', '空调开关', '卫生间', '墙角'},
                           'light switch': {'大门', '空调开关', '卫生间', '墙角'},
                           'airconditioner switch': {'大门', '灯开关', '卫生间', '墙角'},
                           'cabinet': {'灶台', '吧台', '工作台', '服务台', '收银台', '蛋糕柜', '充电处', '冰箱'},
                           'bathroom': {'大门', '墙角'},
                           'window': {'餐桌', '沙发', '休闲区'}, 'audio': {'餐桌', '沙发', '休闲区', '墙角', '书架'},
                           'lounge area': {'沙发', '餐桌', '墙角', '书架', '音响'},
                           'workstation': {'吧台', '服务台', '收银台'},
                           'service counter': {'吧台', '工作台', '收银台'},
                           'cashier counter': {'吧台', '工作台', '服务台'},
                           'corner': {'卫生间', '沙发', '灯开关', '空调开关', '音响', '休闲区', '书架'},
                           'cake display': {'吧台', '橱柜', '服务台', '收银台', '冰箱'},
                           'ChargingStations': {'吧台', '餐桌', '沙发', '休闲区', '工作台', '服务台', '收银台', '墙角',
                                                '书架'},
                           'refrigerator': {'吧台', '服务台', '蛋糕柜'},
                           'bookshelf': {'餐桌', '沙发', '窗户', '休闲区', '墙角'}}

        self.obstacle_objs_id = [114, 115, 122, 96, 102, 83, 121, 105, 108, 89, 100, 90,
                            111, 103, 95, 92, 76, 113, 101, 29, 112, 87, 109, 98,
                            106, 120, 97, 86, 104, 78, 85, 81, 82, 84, 91, 93, 94,
                            99, 107, 116, 117, 118, 119, 255, 251]
        self.not_key_objs_id = {255, 254, 253, 107, 81}

        self.init_algos()  # 初始化各种算法类


    def init_world(self,scene_num=1, mapID=11):
        self.stub.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=mapID))
        time.sleep(3)  # wait for the map to load

    def get_camera(self,part, scene_id=0):
        # print('------------------get_camera----------------------')
        action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id)
        return self.stub.Capture(action)

    def init_robot(self):
        # init robot

        if self.robot:
            self.robot.set_scene(self)
            self.robot.load_BT()

    def init_algos(self):
        '''
            初始化各种各种算法
        '''
        # map_file = os.path.join(root_path, 'robowaiter/algos/navigator/map_5.pkl')
        # with open(map_file, 'rb') as file:
        #     map = pickle.load(file)

        # 初始化探索、导航、操作
        self.navigator = Navigator(scene=self, area_range=[-350, 600, -400, 1450], map=copy.deepcopy(self.map_map_real), scale_ratio=5)
        # self.explorer
        # self.manipulator


    def reset(self):
        # 基类reset，默认执行仿真器初始化操作
        self.reset_sim()
        self.init_robot()
        self.init_algos()

        # reset state
        self.state = copy.deepcopy(self.default_state)

        self._reset()
        print("场景初始化完成")

        self.running = True

    def run(self):
        # 基类run
        self._run()
        # 运行并由robot打印每步信息
        while True:
            self.step()

    def step(self):
        # 基类step，默认执行行为树tick操作
        self.time = time.time() - self.start_time

        # if self.time - self.last_step_time > self.step_interval:
        self.deal_event()
        self.deal_new_event()
        self.deal_signal_event()
        self._step()
        self.robot_changed = self.robot.step()
        self.last_step_time = self.time

    def deal_new_event(self):
        if len(self.new_event_list) > 0:
            next_event = self.new_event_list[0]
            t, func, args = next_event
            if self.time >= t:
                print(f'event: {t}, {func.__name__}')
                self.new_event_list.pop(0)
                func(*args)

    def deal_signal_event(self):
        if len(self.signal_event_list) > 0:
            next_event = self.signal_event_list[0]
            t, func, args = next_event
            if t < 0:  # 一直等待机器人行动，直到机器人无行动
                if self.robot_changed:
                    return
            if (t >= 0) and (self.time - self.last_event_time < t):
                return

            print(f'event: {t}, {func.__name__}')
            self.signal_event_list.pop(0)
            self.last_event_time = self.time
            print("==== *args ======:",*args)
            func(*args)

    def deal_event(self):
        if len(self.event_list) > 0:
            next_event = self.event_list[0]
            t, func = next_event
            if self.time >= t:
                print(f'event: {t}, {func.__name__}')
                self.event_list.pop(0)
                func()

    def create_chat_event(self, sentence):
        def customer_say():
            print(f'{sentence}')
            if self.show_bubble:
                self.chat_bubble(f'{sentence}')
            self.state['chat_list'].append(f'{sentence}')

        return customer_say

    def set_goal(self, goal):
        g = eval("{'" + goal + "'}")

        print("====== g =======:",g)
        def set_sub_task():
            self.state['chat_list'].append(("Goal", g))

        return set_sub_task


    def new_set_goal(self,goal):
        self.state['chat_list'].append(("Goal",goal))


    @property
    def status(self):
        return self.stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID))

    def reset_sim(self):
        # reset world
        self.stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID))
        self.init_world()
        self.stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID))

    def _reset(self):
        # 场景自定义的reset
        pass

    def _run(self):
        # 场景自定义的run
        pass

    def _step(self):
        # 场景自定义的step
        pass

    def walker_control_generator(self, walkerID, autowalk, speed, X, Y, Yaw):
        if self.use_offset:
            X, Y = X + loc_offset[0], Y + loc_offset[1]
        return GrabSim_pb2.WalkerControls.WControl(
            id=walkerID,
            autowalk=autowalk,
            speed=speed,
            pose=GrabSim_pb2.Pose(X=X, Y=Y, Yaw=Yaw),
        )

    def walk_to(self, X, Y, Yaw=100, velocity=200, dis_limit=0):
        walk_v = [X, Y, Yaw, velocity, dis_limit]
        action = GrabSim_pb2.Action(
            scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v
        )
        scene = self.stub.Do(action)

        return scene

    def walker_walk_to(self, walkerID, X, Y, speed=50, Yaw=0):
        self.control_walker(
            [self.walker_control_generator(walkerID=walkerID, autowalk=False, speed=speed, X=X, Y=Y, Yaw=Yaw)])

    def reachable_check(self, X, Y, Yaw):
        if self.use_offset:
            X, Y = X + loc_offset[0], Y + loc_offset[1]
        navigation_info = self.stub.Do(
            GrabSim_pb2.Action(
                scene=self.sceneID,
                action=GrabSim_pb2.Action.ActionType.WalkTo,
                values=[X, Y, Yaw],
            )
        ).info
        if navigation_info == "Unreachable":
            return False
        else:
            return True

    def add_walker(self, id, x, y, yaw=0, v=0, scope=100):
        loc = [x, y, yaw, v, scope]
        action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc)
        scene = self.stub.Do(action)
        # print(scene.info)
        walker_list = []
        if (str(scene.info).find('unreachable') > -1):
            print('当前位置不可达,无法初始化NPC')
        else:
            walker_list.append(
                GrabSim_pb2.WalkerList.Walker(id=id, pose=GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=loc[2])))
        self.stub.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=self.sceneID))

        w = self.status.walkers
        num_customer = len(w)
        name = w[-1].name
        self.state["customer_mem"][name] = num_customer - 1

        self.ui_func(("add_walker",name))


    def walker_index2mem(self, index):
        for mem, i in self.state["customer_mem"].items():
            if index == i:
                return mem

    def add_walkers(self, walker_loc=[[0, 880], [250, 1200], [-55, 750], [70, -200]]):
        print('------------------add_walkers----------------------')
        for i, walker in enumerate(walker_loc):
            if len(walker) == 2:
                self.add_walker(i, walker[0], walker[1])
            elif len(walker) == 3:
                self.add_walker(walker[0], walker[1], walker[2])
            elif len(walker) == 4:
                self.add_walker(walker[0], walker[1], walker[2], walker[3])
            elif len(walker) == 5:
                self.add_walker(walker[0], walker[1], walker[2], walker[3], walker[4])

    def remove_walker(self, *args):  # take single walkerID or a list of walkerIDs
        remove_list = []
        if isinstance(args[0], list):
            remove_list = args[0]
        else:
            for walkerID in args:
                # walkerID is the index of the walker in status.walkers.
                # Since status.walkers is a list, some walkerIDs would change after removing a walker.
                remove_list.append(walkerID)

        self.stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID))
        self.state["customer_mem"] = {}
        w = self.status.walkers
        for i in range(len(w)):
            self.state["customer_mem"][w[i].name] = i

    def remove_walkers(self, IDs=[0]):
        s = self.stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID))
        scene = self.stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=IDs, scene=self.sceneID))
        time.sleep(2)
        self.state["customer_mem"] = {}
        w = self.status.walkers
        for i in range(len(w)):
            self.state["customer_mem"][w[i].name] = i
        return

    def clean_walkers(self):
        scene = self.stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID))
        self.state["customer_mem"] = {}
        return scene

    def control_walker(self, walkerID, autowalk, speed, X, Y, Yaw=0):

        if not isinstance(walkerID, int):
            walkerID = self.walker_index2mem(walkerID)

        pose = GrabSim_pb2.Pose(X=X, Y=Y, Yaw=Yaw)
        scene = self.stub.ControlWalkers(
            GrabSim_pb2.WalkerControls(
                controls=[GrabSim_pb2.WalkerControls.WControl(id=walkerID, autowalk=autowalk, speed=speed, pose=pose)],
                scene=self.sceneID)
        )
        return scene
        # self.stub.ControlWalkers(
        #     GrabSim_pb2.WalkerControls(controls=control_list, scene=self.sceneID)
        # )

    def control_walkers_and_say(self, control_list_ls):
        """ 同时处理行人的行走和对话
        control_list_ls =[walkerID,autowalk,speed,X,Y,Yaw,cont]
        """
        control_list = []
        for control in control_list_ls:
            if control[-1] != None:
                walkerID = control[0]

                if not isinstance(walkerID, int):
                    walkerID = self.walker_index2mem(walkerID)

                # cont = self.status.walkers[walkerID].name + ":"+control[-1]
                # self.control_robot_action(control[walkerID], 3, cont)
                self.customer_say(walkerID, control[-1])
            control_list.append(
                self.walker_control_generator(walkerID=control[0], autowalk=control[1], speed=control[2], X=control[3],
                                              Y=control[4], Yaw=control[5]))
        # 收集没有对话的统一控制
        scene = self.stub.ControlWalkers(
            GrabSim_pb2.WalkerControls(controls=control_list, scene=self.sceneID)
        )
        return scene

    def control_walkers(self, walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]], is_autowalk=True):
        """pose:表示行人的终止位置姿态"""
        scene = self.status
        walker_loc = walker_loc
        controls = []
        for i in range(len(walker_loc)):
            loc = walker_loc[i]
            is_autowalk = is_autowalk
            pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180)
            controls.append(GrabSim_pb2.WalkerControls.WControl(id=i, autowalk=is_autowalk, speed=80, pose=pose))
        scene = self.stub.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=self.sceneID))
        return scene

    def control_walker_ls(self, walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]]):
        """pose:表示行人的终止位置姿态"""
        scene = self.status
        walker_loc = walker_loc
        controls = []
        for walker in walker_loc:
            if len(walker) == 2:
                self.control_walker(walker[0], walker[1])
            elif len(walker) == 3:
                self.control_walker(walker[0], walker[1], walker[2])
            elif len(walker) == 4:
                self.control_walker(walker[0], walker[1], walker[2], walker[3])
            elif len(walker) == 5:
                self.control_walker(walker[0], walker[1], walker[2], walker[3], walker[4])
            elif len(walker) == 6:
                self.control_walker(walker[0], walker[1], walker[2], walker[3], walker[4], walker[5])
        #     self.control_walker()
        # scene = self.stub.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=self.sceneID))
        # return scene
        return

    def control_joints(self, angles):
        self.stub.Do(
            GrabSim_pb2.Action(
                scene=self.sceneID,
                action=GrabSim_pb2.Action.ActionType.RotateJoints,
                values=angles,
            )
        )

    def add_object(self, type, X, Y, Z, Yaw=0):
        if self.use_offset:
            X, Y = X + loc_offset[0], Y + loc_offset[1]
        self.stub.AddObjects(
            GrabSim_pb2.ObjectList(
                objects=[
                    GrabSim_pb2.ObjectList.Object(x=X, y=Y, yaw=Yaw, z=Z, type=type)
                ],
                scene=self.sceneID,
            )
        )

    def remove_object(self, *args):  # refer to remove_walker
        remove_list = []
        if isinstance(args[0], list):
            remove_list = args[0]
        else:
            for objectID in args:
                remove_list.append(objectID)
        self.stub.RemoveObjects(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID))

    def clean_object(self):
        self.stub.CleanObjects(GrabSim_pb2.SceneID(value=self.sceneID))

    def grasp(self, handID, objectID):
        self.stub.Do(
            GrabSim_pb2.Action(
                scene=self.sceneID,
                action=GrabSim_pb2.Action.ActionType.Grasp,
                values=[handID, objectID],
            )
        )

    def release(self, handID):
        self.stub.Do(
            GrabSim_pb2.Action(
                scene=self.sceneID,
                action=GrabSim_pb2.Action.ActionType.Release,
                values=[handID],
            )
        )

    def get_camera_color(self, image_only=True):
        camera_data = self.stub.Capture(
            GrabSim_pb2.CameraList(
                cameras=[GrabSim_pb2.CameraName.Head_Color], scene=self.sceneID
            )
        )
        if image_only:
            return show_image(camera_data)
        else:
            return camera_data

    def get_camera_depth(self, image_only=True):
        camera_data = self.stub.Capture(
            GrabSim_pb2.CameraList(
                cameras=[GrabSim_pb2.CameraName.Head_Depth], scene=self.sceneID
            )
        )
        if image_only:
            return show_image(camera_data)
        else:
            return camera_data

    def get_camera_segment(self, show=True):
        camera_data = self.stub.Capture(
            GrabSim_pb2.CameraList(
                cameras=[GrabSim_pb2.CameraName.Head_Segment], scene=self.sceneID
            )
        )
        if show:
            show_image(camera_data)

        return camera_data

    def chat_bubble(self, message):
        self.stub.ControlRobot(
            GrabSim_pb2.ControlInfo(
                scene=self.sceneID, type=0, action=1, content=message #message.strip()
            )
        )

    def walker_bubble(self, name, message):
        talk_content = name + ":" + message
        self.control_robot_action(0, 3, talk_content)

    def customer_say(self, name, sentence, show_bubble=True):
        if isinstance(name, int):
            name = self.walker_index2mem(name)

        # if not isinstance(walkerID, int):
        #     name = self.walker_index2mem(walkerID)

        print(f'{name} say: {sentence}')
        if self.show_bubble and show_bubble:
            self.walker_bubble(name, sentence)
        self.state['chat_list'].append((name, sentence))

    # def control_robot_action(self, scene_id=0, type=0, action=0, message="你好"):
    #     print('------------------control_robot_action----------------------')
    #     scene = self.stub.ControlRobot(
    #         GrabSim_pb2.ControlInfo(scene=scene_id, type=type, action=action, content=message))
    #     if (str(scene.info).find("Action Success") > -1):
    #         print(scene.info)
    #         return True
    #     else:
    #         print(scene.info)
    #         return False

    def animation_control(self, animation_type):
        # animation_type: 1:make coffee 2: pour water 3: grab food 4: mop floor 5: clean table
        scene = self.stub.ControlRobot(
            GrabSim_pb2.ControlInfo(scene=self.sceneID, type=animation_type, action=1)
        )
        if scene.info == "action success":
            for i in range(2, animation_step[animation_type - 1] + 1):
                self.stub.ControlRobot(
                    GrabSim_pb2.ControlInfo(
                        scene=self.sceneID, type=animation_type, action=i
                    )
                )

    def animation_reset(self):
        self.stub.ControlRobot(GrabSim_pb2.ControlInfo(scene=self.sceneID, type=0, action=0))

    # 手指移动到指定位置
    def ik_control_joints(self, handNum=2, x=30, y=40, z=80):
        # print('------------------ik_control_joints----------------------')
        # IK控制,双手, 1左手, 2右手; 暂时只动右手
        HandPostureObject = [
            GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=handNum, x=x, y=y, z=z, roll=0, pitch=0, yaw=0),
            # GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=1, x=0, y=0, z=0, roll=0, pitch=0, yaw=0),
        ]
        temp = self.stub.GetIKControlInfos(
            GrabSim_pb2.HandPostureInfos(scene=self.sceneID, handPostureObjects=HandPostureObject))

    def move_to_obj(self, obj_id):

        scene = self.status
        # 抬头
        # value = [0]*21
        # for i in range(21):
        #     value[i] = self.status.joints[i].angle
        # value[5] = 0
        # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
        # scene = self.stub.Do(action)
        # time.sleep(1.0)

        obj_info = scene.objects[obj_id]
        # Robot
        obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z
        walk_v = [obj_x + 50, obj_y] + [180, 180, 0]
        if obj_y >= 820 and obj_y <= 1200 and obj_x >= 240 and obj_x <= 500:  # 物品位于斜的抹布桌上 ([240,500],[820,1200])
            walk_v = [obj_x + 40, obj_y - 35, 130, 180, 0]
            obj_x += 3
            obj_y += 2.5
        walk_v[0] += 1
        print("walk:", walk_v)
        if self.is_nav_walk:
            self.navigator.navigate(goal=(walk_v[0], walk_v[1]), animation=False)
        else:
            action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
            scene = self.stub.Do(action)
        print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw])

    # 移动到进行操作任务的指定地点
    def move_task_area(self, op_type, obj_id=0, release_pos=[247.0, 520.0, 100.0]):
        scene = self.status

        # 抬头
        # value = [0]*21
        # for i in range(21):
        #     value[i] = self.status.joints[i].angle
        # value[5] = 0
        # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
        # scene = self.stub.Do(action)
        # time.sleep(1.0)

        cur_pos = [scene.location.X, scene.location.Y, scene.rotation.Yaw]
        print("Current Position:", cur_pos, "开始任务:", self.op_dialog[op_type])
        if op_type == 11 or op_type == 12:  # 开关窗帘不需要移动
            return
        print('------------------moveTo_Area----------------------')
        if op_type < 8:  # 动画控制相关任务的移动目标
            if self.show_ui:
                self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
            walk_v = self.op_v_list[op_type] + [scene.rotation.Yaw, 180, 0]
        if 8 <= op_type <= 10:  # 控灯相关任务的移动目标
            if self.show_ui:
                self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
            walk_v = self.op_v_list[6] + [scene.rotation.Yaw, 180, 0]
        if op_type in [13, 14, 15]:  # 空调相关任务的移动目标
            if self.show_ui:
                self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
            walk_v = [240, -140.0] + [0, 180, 0]
        if op_type == 16:  # 抓握物体，移动到物体周围的可达区域
            scene = self.status
            obj_info = scene.objects[obj_id]
            obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z
            walk_v = [obj_x + 50, obj_y] + [180, 180, 0]
            if obj_info.name == 'Plate':
                walk_v = [obj_x + 51, obj_y] + [180, 180, 0]
            if 820 <= obj_y <= 1200 and 240 <= obj_x <= 500:  # 物品位于斜的抹布桌上 ([240,500],[820,1200])
                walk_v = [obj_x + 40, obj_y - 35, 130, 180, 0]
        if op_type == 17:  # 放置物体，移动到物体周围的可达区域
            walk_v = release_pos[:-1] + [180, 180, 0]
            if release_pos == [340.0, 900.0, 99.0]:
                walk_v[2] = 130
        # 移动到目标位置
        action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
        scene = self.stub.Do(action)
        print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw])

    # 相应的行动，由主办方封装
    def control_robot_action(self, type=0, action=0, message="你好"):
        scene = self.stub.ControlRobot(
            GrabSim_pb2.ControlInfo(
                scene=self.sceneID, type=type, action=action, content=message
            )
        )
        if str(scene.info).find("Action Success") > -1:
            print(scene.info)
            return True
        else:
            print(scene.info)
            return False

    # 调整空调开关、温度
    def adjust_kongtiao(self, op_type):
        # 低头
        value = [0] * 21
        for i in range(21):
            value[i] = self.status.joints[i].angle
        value[5] = 30
        action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
        scene = self.stub.Do(action)
        time.sleep(1.0)

        if self.show_ui:
            self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio,update_info_count=1)


        obj_loc = self.obj_loc[:]
        obj_loc[2] -= 5
        if op_type == 13: obj_loc[1] -= 2
        if op_type == 14: obj_loc[1] -= 0
        if op_type == 15: obj_loc[1] += 2
        self.ik_control_joints(2, obj_loc[0], obj_loc[1], obj_loc[2])
        time.sleep(3.0)
        self.robo_recover()  # 恢复肢体关节
        return True

    def gen_obj_tmp(self, h=100):
        # 4;冰红(盒) 5;酸奶  7:保温杯 9;冰红(瓶) 13:代语词典  14:cake 61:甜牛奶
        scene = self.status
        ginger_loc = [scene.location.X, scene.location.Y, scene.location.Z]
        obj_list = [

            GrabSim_pb2.ObjectList.Object(x=190, y=40, z=87, roll=0, pitch=0, yaw=0,
                                          type=38), #矿泉水


            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 75, z=95, roll=0, pitch=0, yaw=0,
                                          type=48),  # 48是薯片
            # GrabSim_pb2.ObjectList.Object(x=190, y=40, z=87, roll=0, pitch=0, yaw=0,
            #                               type=48), #48是薯片
            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 65, z=95, roll=0, pitch=0, yaw=0,
                                          type=37), #37是NFC果汁
            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 55, z=95, roll=0, pitch=0, yaw=0,
                                          type=8), #8是贝尔纳松
            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 45, z=95, roll=0, pitch=0, yaw=0,
                                          type=6), #6是AD钙奶
            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 35, z=95, roll=0, pitch=0, yaw=0,
                                          type=9), #9是冰红(瓶)
            # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 25, z=95, roll=0, pitch=0, yaw=0,
            #                               type=7),  # 5是酸奶

            # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 30, z=95, roll=0, pitch=0, yaw=0,
            #                               type=13),
            # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 40, z=95, roll=0, pitch=0, yaw=0,
            #                               type=48),
            # GrabSim_pb2.ObjectList.Object(x=340, y=960, z=88, roll=0, pitch=0, yaw=90, type=7),
            # GrabSim_pb2.ObjectList.Object(x=340, y=960, z = 88, roll=0, pitch=0, yaw=90, type=9),

            GrabSim_pb2.ObjectList.Object(x=320, y=400, z=95, roll=0, pitch=0, yaw=0,
                                          type=20),

            # 斜桌三瓶冰红茶
            # GrabSim_pb2.ObjectList.Object(x=340, y=965, z=88, roll=0, pitch=0, yaw=90, type=4),
            # GrabSim_pb2.ObjectList.Object(x=320, y=940, z=88, roll=0, pitch=0, yaw=90, type=4),
            GrabSim_pb2.ObjectList.Object(x=300, y=930, z=88, roll=0, pitch=0, yaw=90, type=5),
            # GrabSim_pb2.ObjectList.Object(x=300, y=930, z=88, roll=0, pitch=0, yaw=90, type=38), #矿泉水

            GrabSim_pb2.ObjectList.Object(x=370, y=1000, z=88, roll=0, pitch=0, yaw=90, type=1), #香蕉
            GrabSim_pb2.ObjectList.Object(x=380, y=1000, z=88, roll=0, pitch=0, yaw=90, type=65),  # 番茄
            GrabSim_pb2.ObjectList.Object(x=380, y=1020, z=88, roll=0, pitch=0, yaw=90, type=42),  # 山竹
            GrabSim_pb2.ObjectList.Object(x=360, y=1020, z=88, roll=0, pitch=0, yaw=90, type=27),  # 橙子

            # BrightTable2
            # GrabSim_pb2.ObjectList.Object(x=-30, y=1000, z=35, roll=0, pitch=0, yaw=90, type=64), #西瓜
            GrabSim_pb2.ObjectList.Object(x=-15, y=1050, z=40, roll=0, pitch=0, yaw=90, type=17), #a午餐盒

            # 保温杯
            GrabSim_pb2.ObjectList.Object(x=-102, y=10, z=90, roll=0, pitch=0, yaw=90, type=7),
            # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 70, z=95, roll=0, pitch=0, yaw=0,
            #                               type=9),

            # Table3上由两套军旗，一个模仿
            GrabSim_pb2.ObjectList.Object(x=-115, y=200, z=85, roll=0, pitch=0, yaw=90, type=26), # Chess
            GrabSim_pb2.ObjectList.Object(x=-130, y=225, z=85, roll=0, pitch=0, yaw=90, type=55), # 玩具狗
            GrabSim_pb2.ObjectList.Object(x=-110, y=225, z=85, roll=0, pitch=0, yaw=90, type=56), #玩具熊
            GrabSim_pb2.ObjectList.Object(x=-115, y=250, z=85, roll=0, pitch=0, yaw=90, type=26),  # Chess
            GrabSim_pb2.ObjectList.Object(x=-115, y=280, z=85, roll=0, pitch=0, yaw=90, type=35),  # 魔方

            # 靠窗边的桌子上
            GrabSim_pb2.ObjectList.Object(x=-400, y=520, z=70, roll=0, pitch=0, yaw=0, type=63),  # 小说
            GrabSim_pb2.ObjectList.Object(x=-410, y=550, z=70, roll=0, pitch=0, yaw=0, type=59),  # 围巾
            GrabSim_pb2.ObjectList.Object(x=-395, y=570, z=70, roll=0, pitch=0, yaw=0, type=18), # 手镯


        ]
        scene = self.stub.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=self.sceneID))
        time.sleep(1.0)





    def gen_obj(self, h=100):
        # 4;冰红(盒) 5;酸奶  7:保温杯 9;冰红(瓶) 13:代语词典  14:cake 61:甜牛奶
        scene = self.status
        ginger_loc = [scene.location.X, scene.location.Y, scene.location.Z]
        obj_list = [

            GrabSim_pb2.ObjectList.Object(x=190, y=40, z=87, roll=0, pitch=0, yaw=0,
                                          type=38), #矿泉水


            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 75, z=95, roll=0, pitch=0, yaw=0,
                                          type=48),  # 48是薯片
            # GrabSim_pb2.ObjectList.Object(x=190, y=40, z=87, roll=0, pitch=0, yaw=0,
            #                               type=48), #48是薯片
            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 65, z=95, roll=0, pitch=0, yaw=0,
                                          type=37), #37是NFC果汁
            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 55, z=95, roll=0, pitch=0, yaw=0,
                                          type=8), #8是贝尔纳松
            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 45, z=95, roll=0, pitch=0, yaw=0,
                                          type=6), #6是AD钙奶
            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 35, z=95, roll=0, pitch=0, yaw=0,
                                          type=9), #9是冰红(瓶)
            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 25, z=95, roll=0, pitch=0, yaw=0,
                                          type=5),  # 5是酸奶

            # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 30, z=95, roll=0, pitch=0, yaw=0,
            #                               type=13),
            # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 40, z=95, roll=0, pitch=0, yaw=0,
            #                               type=48),
            # GrabSim_pb2.ObjectList.Object(x=340, y=960, z=88, roll=0, pitch=0, yaw=90, type=7),
            # GrabSim_pb2.ObjectList.Object(x=340, y=960, z = 88, roll=0, pitch=0, yaw=90, type=9),

            GrabSim_pb2.ObjectList.Object(x=320, y=400, z=95, roll=0, pitch=0, yaw=0,
                                          type=20),

            # 斜桌三瓶冰红茶
            GrabSim_pb2.ObjectList.Object(x=340, y=965, z=88, roll=0, pitch=0, yaw=90, type=4),
            GrabSim_pb2.ObjectList.Object(x=320, y=940, z=88, roll=0, pitch=0, yaw=90, type=4),
            # GrabSim_pb2.ObjectList.Object(x=300, y=930, z=88, roll=0, pitch=0, yaw=90, type=4),
            # GrabSim_pb2.ObjectList.Object(x=300, y=930, z=88, roll=0, pitch=0, yaw=90, type=38), #矿泉水

            GrabSim_pb2.ObjectList.Object(x=370, y=1000, z=88, roll=0, pitch=0, yaw=90, type=1), #香蕉
            GrabSim_pb2.ObjectList.Object(x=380, y=1000, z=88, roll=0, pitch=0, yaw=90, type=65),  # 番茄
            GrabSim_pb2.ObjectList.Object(x=380, y=1020, z=88, roll=0, pitch=0, yaw=90, type=42),  # 山竹
            GrabSim_pb2.ObjectList.Object(x=360, y=1020, z=88, roll=0, pitch=0, yaw=90, type=27),  # 橙子

            # BrightTable2
            # GrabSim_pb2.ObjectList.Object(x=-30, y=1000, z=35, roll=0, pitch=0, yaw=90, type=64), #西瓜
            GrabSim_pb2.ObjectList.Object(x=-15, y=1050, z=40, roll=0, pitch=0, yaw=90, type=17), #a午餐盒

            # 保温杯
            GrabSim_pb2.ObjectList.Object(x=-102, y=10, z=90, roll=0, pitch=0, yaw=90, type=7),
            # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 70, z=95, roll=0, pitch=0, yaw=0,
            #                               type=9),

            # Table3上由两套军旗，一个模仿
            GrabSim_pb2.ObjectList.Object(x=-115, y=200, z=85, roll=0, pitch=0, yaw=90, type=26), # Chess
            GrabSim_pb2.ObjectList.Object(x=-130, y=225, z=85, roll=0, pitch=0, yaw=90, type=55), # 玩具狗
            GrabSim_pb2.ObjectList.Object(x=-110, y=225, z=85, roll=0, pitch=0, yaw=90, type=56), #玩具熊
            GrabSim_pb2.ObjectList.Object(x=-115, y=250, z=85, roll=0, pitch=0, yaw=90, type=26),  # Chess
            GrabSim_pb2.ObjectList.Object(x=-115, y=280, z=85, roll=0, pitch=0, yaw=90, type=35),  # 魔方

            # 靠窗边的桌子上
            GrabSim_pb2.ObjectList.Object(x=-400, y=520, z=70, roll=0, pitch=0, yaw=0, type=63),  # 小说
            GrabSim_pb2.ObjectList.Object(x=-410, y=550, z=70, roll=0, pitch=0, yaw=0, type=59),  # 围巾
            GrabSim_pb2.ObjectList.Object(x=-395, y=570, z=70, roll=0, pitch=0, yaw=0, type=18), # 手镯


        ]
        scene = self.stub.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=self.sceneID))
        time.sleep(1.0)



    # 实现抓握操作
    def grasp_obj(self, obj_id, hand_id=1):
        print('------------------adjust_joints----------------------')
        scene = self.status

        # 低头
        value = [0] * 21
        for i in range(21):
            value[i] = self.status.joints[i].angle
        value[5] = 30
        action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
        scene = self.stub.Do(action)
        time.sleep(1.0)

        if self.show_ui:
            self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)

        obj_info = scene.objects[obj_id]
        obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z
        if 820 <= obj_y <= 1200 and 240 <= obj_x <= 500: # 物品位于斜的抹布桌上 ([240,500],[820,1200])
            obj_x += 3
            obj_y += 2.5
        if obj_info.name == "CoffeeCup":
            # obj_x += 2.5
            # obj_y -= 0.7  # 1.7
            # obj_z -= 6
            # values= [0, 0, 0, 0, 0, 15, -6, -6, -6, -6]  # 后5位右手 [-6,45]
            # stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values))
            pass
        if obj_info.name == "Glass":
            pass
        # Finger
        self.ik_control_joints(2, obj_x - 9, obj_y, obj_z)  # -10, 0, 0
        time.sleep(3.0)
        # Grasp Obj
        print('------------------grasp_obj----------------------')
        action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Grasp,
                                    values=[hand_id, obj_id])
        scene = self.stub.Do(action)
        time.sleep(3.0)
        return True

    # robot的肢体恢复原位
    def robo_recover(self):
        action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints,  # 恢复原位
                                    values=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
        scene = self.stub.Do(action)

    # 恢复手指关节
    def standard_finger(self):
        values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        self.stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values))
        time.sleep(1.0)

    # 弯腰以及手掌与放置面平齐
    def robo_stoop_parallel(self):
        # 0-3是躯干，4-6是脖子和头，7-13是左胳膊，14-20是右胳膊
        scene = self.status
        angle = [scene.joints[i].angle for i in range(21)]
        angle[0] = 15  # 15
        angle[19] = -15
        angle[20] = -30
        action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints,  # 弯腰
                                    values=angle)
        scene = self.stub.Do(action)
        time.sleep(1.0)

    # 实现放置操作
    def release_obj(self, release_pos):
        print("------------------adjust_joints----------------------")
        # 低头
        value = [0] * 21
        for i in range(21):
            value[i] = self.status.joints[i].angle
        value[5] = 30
        action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
        scene = self.stub.Do(action)
        time.sleep(1.0)

        if self.show_ui:
            self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)

        if release_pos == [340.0, 900.0, 99.0]:
            self.ik_control_joints(2, release_pos[0] - 40, release_pos[1] + 35, release_pos[2])
            time.sleep(2.0)
        else:
            self.ik_control_joints(2, release_pos[0] - 80, release_pos[1], release_pos[2])
            time.sleep(2.0)
            self.robo_stoop_parallel()
        print("------------------release_obj----------------------")
        action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Release, values=[1])
        scene = self.stub.Do(action)
        time.sleep(2.0)
        self.robo_recover()  # 恢复肢体关节
        self.standard_finger()  # 恢复手指关节
        return True

    # 执行过程: Robot输出"开始(任务名)" -> 按步骤数执行任务 -> Robot输出成功或失败的对话
    def op_task_execute(self, op_type, obj_id=0, release_pos=[247.0, 520.0, 100.0]):
        #id = 196  # Glass = 188+x, Plate = 150+x
        self.control_robot_action(0, 1, "Start " + self.op_dialog[op_type])  # 输出正在执行的任务
        if op_type < 8:
            if self.show_ui:
                self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
            result = self.control_robot_action(op_type, 1)
        if 8 <= op_type <= 12:
            if self.show_ui:
                self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
            result = self.control_robot_action(self.op_typeToAct[op_type][0], self.op_typeToAct[op_type][1])
        if op_type in [13, 14, 15]:  # 调整空调:13代表按开关,14升温,15降温
            result = self.adjust_kongtiao(op_type)
        if op_type == 16:  # 抓握物体, 需要传入物品id
            result = self.grasp_obj(obj_id)
        if op_type == 17:  # 放置物体, 放置物品, 需要传入放置地点
            result = self.release_obj(release_pos)
        self.control_robot_action(0, 2)
        if result:
            if self.op_act_num[op_type] > 0:
                for i in range(2, 2 + self.op_act_num[op_type]):
                    self.control_robot_action(op_type, i)
                    self.control_robot_action(0, 2)
            # self.control_robot_action(0, 1, "成功"+self.op_dialog[op_type])
        # else:
        #     self.control_robot_action(0, 1, self.op_dialog[op_type]+"失败")

    def find_obj(self, name):
        for id, item in enumerate(self.status.objects):
            if item.name == name:
                print("name:", name, "id:", id, "X:", item.location.X, "Y:", item.location.Y, "Z:", item.location.Z, )

    def test_move(self):
        v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]]
        scene = self.status
        for walk_v in v_list:
            walk_v = walk_v + [scene.rotation.Yaw - 90, 600, 100]
            print("walk_v", walk_v)
            action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
            scene = self.stub.Do(action)
            print(scene.info)

    def navigation_move(self, plt, cur_objs, cur_obstacle_world_points, v_list, map_ratio, db, scene_id=0, map_id=11):
        print('------------------navigation_move----------------------')
        scene = self.stub.Observe(GrabSim_pb2.SceneID(value=scene_id))
        walk_value = [scene.location.X, scene.location.Y]
        print("position:", walk_value)

        if not cur_objs:
            walk_v = [scene.location.X, scene.location.Y + 1]
            yaw = Navigator.get_yaw(walk_value, walk_v)
            yaw =  math.degrees(yaw)
            walk_v = walk_value + [yaw, 250, 10]
            print("walk_v", walk_v)
            action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
            scene = self.stub.Do(action)
            # cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
            #                                                   objs_name_set)

            # cur_obstacle_world_points, cur_objs_id = camera.get_obstacle_point(plt, db, scene,
            #                                                                    cur_obstacle_world_points, map_ratio)
            cur_obstacle_world_points, cur_objs_id, obj_detect_count= camera.get_obstacle_point(self, db, scene,
                                                                               cur_obstacle_world_points, map_ratio)
            # cur_obstacle_world_points, cur_objs_id = self.get_obstacle_point(db, scene, map_ratio)
            # # self.get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio)


            # if scene.info == "Unreachable":
            print(scene.info)

        # if map_id == 11:  # coffee
        #     v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]]
        # else:
        #     v_list = [[0.0, 0.0]]

        else:
            for walk_v in v_list:
                yaw = Navigator.get_yaw(walk_value, walk_v)
                yaw =  math.degrees(yaw)
                walk_v = walk_v + [yaw, 250, 10]
                print("walk_v", walk_v)
                action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
                scene = self.stub.Do(action)

                # cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
                #                                                   objs_name_set)

                cur_obstacle_world_points, cur_objs_id, obj_detect_count= camera.get_obstacle_point(self, db, scene,
                                                                                   cur_obstacle_world_points, map_ratio)

                # if scene.info == "Unreachable":
                print(scene.info)
        return cur_obstacle_world_points, cur_objs_id, obj_detect_count

    def isOutMap(self, pos, min_x=-200, max_x=600, min_y=-250, max_y=1300):
        if pos[0] <= min_x or pos[0] >= max_x or pos[1] <= min_y or pos[1] >= max_y:
            return True
        return False

    def real2map(self, x, y):
        '''
            实际坐标->地图坐标 (向下取整)
        '''
        # x = round((x - self.min_x) / self.scale_ratio)
        # y = round((y - self.min_y) / self.scale_ratio)
        x = math.floor((x + 200))
        y = math.floor((y + 250))
        return x, y

    def explore(self, map, explore_range):
        scene = self.stub.Observe(GrabSim_pb2.SceneID(value=0))
        cur_pos = [int(scene.location.X), int(scene.location.Y)]
        for i in range(cur_pos[0] - explore_range, cur_pos[0] + explore_range + 1):
            for j in range(cur_pos[1] - explore_range, cur_pos[1] + explore_range + 1):
                if self.isOutMap((i, j)):
                    continue
                x, y = self.real2map(i, j)
                if map[x, y] == 0:
                    self.visited.add((i, j))
                    self.auto_map[x][y] = 0
        for i in range(cur_pos[0] - explore_range, cur_pos[0] + explore_range + 1):
            for j in range(cur_pos[1] - explore_range, cur_pos[1] + explore_range + 1):
                if self.isOutMap((i, j)):
                    continue
                x, y = self.real2map(i, j)
                if map[x, y] == 0:
                    if self.isNewFrontier((i, j), map):
                        self.all_frontier_list.add((i, j))
        if len(self.all_frontier_list) == 0:
            free_list = list(self.visited)
            free_array = np.array(free_list)
            print(f"主动探索完成！保存了二维地图与环境中重点物品语义信息！")

            # # 画地图: X行Y列，第一行在下面
            # plt.clf()
            # plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower',
            #            extent=(-250, 1300,
            #                    -200, 600))
            # plt.show()
            # print("已绘制完成地图！！！")

            return None
        # # 画地图: X行Y列，第一行在下面
        # plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower',
        #            extent=(-250, 1300,
        #                    -200, 600))
        # plt.show()
        # print("已绘制部分地图！")
        return self.getNearestFrontier(cur_pos, self.all_frontier_list)

    def isNewFrontier(self, pos, map):
        around_nodes = [(pos[0], pos[1] + 1), (pos[0], pos[1] - 1), (pos[0] - 1, pos[1]), (pos[0] + 1, pos[1])]

        for node in around_nodes:
            x, y = self.real2map(node[0], node[1])
            if not self.isOutMap((node[0], node[1])) and node not in self.visited and map[x, y] == 0:
                return True
        if (pos[0], pos[1]) in self.all_frontier_list:
            self.all_frontier_list.remove((pos[0], pos[1]))
        return False

    def getDistance(self, pos1, pos2):
        return math.sqrt((pos1[0] - pos2[0]) ** 2 + (pos1[1] - pos2[1]) ** 2)

    def getDistanc3D(self, pos1, pos2):
        return math.sqrt((pos1[0] - pos2[0]) ** 2 + (pos1[1] - pos2[1]) ** 2+ (pos1[2] - pos2[2]) ** 2)


    def getNearestFrontier(self, cur_pos, frontiers):
        dis_min = sys.maxsize
        frontier_best = None
        for frontier in frontiers:
            dis = self.getDistance(frontier, cur_pos)
            if dis <= dis_min:
                dis_min = dis
                frontier_best = frontier
        return frontier_best

    def cal_distance_to_robot(self, objx, objy, objz):
        scene = self.status
        ginger_x, ginger_y, ginger_z = [int(scene.location.X), int(scene.location.Y), 100]
        return math.sqrt((ginger_x - objx) ** 2 + (ginger_y - objy) ** 2 + (ginger_z - objz) ** 2)

    # 根据map文件判断是否可达
    def reachable(self, pos):
        x, y = self.real2map(pos[0], pos[1])
        if self.map_file[x, y] == 0:
            return True
        else:
            return False


    def transform_co(self,img_data, pixel_x_, pixel_y_, depth_, id=0, label=0):
        im = img_data.images[0]

        # 相机外参矩阵
        out_matrix = np.array(im.parameters.matrix).reshape((4, 4))

        d = np.frombuffer(im.data, dtype=im.dtype).reshape((im.height, im.width, im.channels))
        depth = depth_

        # 将像素坐标转换为归一化设备坐标
        normalized_x = (pixel_x_ - im.parameters.cx) / im.parameters.fx
        normalized_y = (pixel_y_ - im.parameters.cy) / im.parameters.fy

        # 将归一化设备坐标和深度值转换为相机坐标
        camera_x = normalized_x * depth
        camera_y = normalized_y * depth
        camera_z = depth

        # 构建相机坐标向量
        camera_coordinates = np.array([camera_x, camera_y, camera_z, 1])
        # print("物体相对相机坐标的齐次坐标: ", camera_coordinates)

        # 将相机坐标转换为机器人底盘坐标
        robot_coordinates = np.dot(out_matrix, camera_coordinates)[:3]
        # print("物体的相对底盘坐标为:", robot_coordinates)

        # 将物体相对机器人底盘坐标转为齐次坐标
        robot_homogeneous_coordinates = np.array([robot_coordinates[0], -robot_coordinates[1], robot_coordinates[2], 1])
        # print("物体的相对底盘的齐次坐标为:", robot_homogeneous_coordinates)

        # 机器人坐标
        X = self.status.location.X
        Y = self.status.location.Y
        Z = 0.0

        # 机器人旋转信息
        Roll = 0.0
        Pitch = 0.0
        Yaw = self.status.rotation.Yaw

        # 构建平移矩阵
        T = np.array([[1, 0, 0, X],
                      [0, 1, 0, Y],
                      [0, 0, 1, Z],
                      [0, 0, 0, 1]])

        # 构建旋转矩阵
        Rx = np.array([[1, 0, 0, 0],
                       [0, np.cos(Roll), -np.sin(Roll), 0],
                       [0, np.sin(Roll), np.cos(Roll), 0],
                       [0, 0, 0, 1]])

        Ry = np.array([[np.cos(Pitch), 0, np.sin(Pitch), 0],
                       [0, 1, 0, 0],
                       [-np.sin(Pitch), 0, np.cos(Pitch), 0],
                       [0, 0, 0, 1]])

        Rz = np.array([[np.cos(np.radians(Yaw)), -np.sin(np.radians(Yaw)), 0, 0],
                       [np.sin(np.radians(Yaw)), np.cos(np.radians(Yaw)), 0, 0],
                       [0, 0, 1, 0],
                       [0, 0, 0, 1]])

        R = np.dot(Rz, np.dot(Ry, Rx))

        # 构建机器人的变换矩阵
        T_robot = np.dot(T, R)
        # print(T_robot)

        # 将物体的坐标从机器人底盘坐标系转换到世界坐标系
        world_coordinates = np.dot(T_robot, robot_homogeneous_coordinates)[:3]

        # if world_coordinates[0] < 200 and world_coordinates[1] <= 1050:
        #     world_coordinates[0] += 400
        #     world_coordinates[1] += 400
        # elif world_coordinates[0] >= 200 and world_coordinates[1] <= 1050:
        #     world_coordinates[0] -= 550
        #     world_coordinates[1] += 400
        # elif world_coordinates[0] >= 200 and world_coordinates[1] > 1050:
        #     world_coordinates[0] -= 550
        #     world_coordinates[1] -= 1450
        # elif world_coordinates[0] < 200 and world_coordinates[1] > 1050:
        #     world_coordinates[0] += 400
        #     world_coordinates[1] -= 1450
        # print("物体的世界坐标：", world_coordinates)

        # 世界偏移后的坐标
        world_offest_coordinates = [world_coordinates[0] + 700, world_coordinates[1] + 1400, world_coordinates[2]]
        # print("物体世界偏移的坐标: ", world_offest_coordinates)
        return world_coordinates

    def ui_func(self,args):
        plt.show()

    def draw_current_bt(self):
        pass

    def get_obstacle_point(self, db, scene, map_ratio, update_info_count=0, is_nav=False):

        # if abs(self.last_take_pic_tim - self.time)<

        # db = DBSCAN(eps=4, min_samples=2)
        cur_obstacle_pixel_points = []
        cur_obstacle_world_points = []
        obj_detect_count = 0
        walker_detect_count = 0
        object_pixels = {}

        not_key_objs_id = {255, 254, 253, 107, 81}


        img_data_segment,img_data_depth,img_data_color = self.get_cameras()
        if len(img_data_segment.images) <1:
            return
        im_segment = img_data_segment.images[0]
        im_depth = img_data_depth.images[0]
        im_color = img_data_color.images[0]

        d_segment = np.frombuffer(im_segment.data, dtype=im_segment.dtype).reshape(
            (im_segment.height, im_segment.width, im_segment.channels))
        d_depth = np.frombuffer(im_depth.data, dtype=im_depth.dtype).reshape(
            (im_depth.height, im_depth.width, im_depth.channels))
        d_color = np.frombuffer(im_color.data, dtype=im_color.dtype).reshape(
            (im_color.height, im_color.width, im_color.channels))


        items = img_data_segment.info.split(";")
        objs_id = {}
        for item in items:
            key, value = item.split(":")
            objs_id[int(key)] = value
        objs_id[251] = "walker"
        # plt.imshow(d_depth, cmap="gray" if "depth" in im_depth.name.lower() else None)
        # plt.show()
        img_segment = d_segment


        d_depth = np.transpose(d_depth, (1, 0, 2))
        d_segment = np.transpose(d_segment, (1, 0, 2))

        for i in range(0, d_segment.shape[0], map_ratio):
            for j in range(0, d_segment.shape[1], map_ratio):
                if d_depth[i][j][0] == 600:
                    continue

                # if d_segment[i][j] == 96:
                #     print(f"apple的像素坐标：({i},{j})")
                #     print(f"apple的深度：{d_depth[i][j][0]}")
                #     print(f"apple的世界坐标: {transform_co(img_data_depth, i, j, d_depth[i][j][0], scene)}")
                # if d_segment[i][j] == 113:
                #     print(f"kettle的像素坐标：({i},{j})")
                #     print(f"kettle的深度：{d_depth[i][j][0]}")
                #     print(f"kettle的世界坐标: {transform_co(img_data_depth, i, j, d_depth[i][j][0], scene)}")
                if d_segment[i][j][0] in [251]:
                    cur_obstacle_pixel_points.append([i, j])
                if d_segment[i][j][0] not in not_key_objs_id:
                    # 首先检查键是否存在
                    if d_segment[i][j][0] in object_pixels:
                        # 如果键存在，那么添加元组(i, j)到对应的值中
                        object_pixels[d_segment[i][j][0]].append([i, j])
                    else:
                        # 如果键不存在，那么创建一个新的键值对，其中键是d_segment[i][j][0]，值是一个包含元组(i, j)的列表
                        object_pixels[d_segment[i][j][0]] = [[i, j]]

        for i in range(0, d_segment.shape[0], map_ratio):
            for j in range(0, d_segment.shape[1], map_ratio):
                if d_depth[i][j][0] == 600:
                    continue

                # if d_segment[i][j] == 96:
                #     print(f"apple的像素坐标：({i},{j})")
                #     print(f"apple的深度：{d_depth[i][j][0]}")
                #     print(f"apple的世界坐标: {transform_co(img_data_depth, i, j, d_depth[i][j][0], scene)}")
                # if d_segment[i][j] == 113:
                #     print(f"kettle的像素坐标：({i},{j})")
                #     print(f"kettle的深度：{d_depth[i][j][0]}")
                #     print(f"kettle的世界坐标: {transform_co(img_data_depth, i, j, d_depth[i][j][0], scene)}")
                # if d_segment[i][j][0] in [251]:
                #     cur_obstacle_pixel_points.append([i, j])
                if d_segment[i][j][0] not in not_key_objs_id:
                    # 首先检查键是否存在
                    if d_segment[i][j][0] in object_pixels:
                        # 如果键存在，那么添加元组(i, j)到对应的值中
                        object_pixels[d_segment[i][j][0]].append([i, j])
                    else:
                        # 如果键不存在，那么创建一个新的键值对，其中键是d_segment[i][j][0]，值是一个包含元组(i, j)的列表
                        object_pixels[d_segment[i][j][0]] = [[i, j]]


        # print(cur_obstacle_pixel_points)
        # for pixel in cur_obstacle_pixel_points:
        #     world_point = self.transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0], scene)
        #     cur_obstacle_world_points.append([world_point[0], world_point[1]])
        # print(f"{pixel}：{[world_point[0], world_point[1]]}")
        img_obj = d_color

        # self.ui_func(("draw_img","img_label_obj",d_color))

        # 画分隔图
        # plt.subplot(2, 2, 1)
        plt.figure()
        plt.imshow(img_segment, cmap="gray" if "depth" in im_segment.name.lower() else None)
        plt.axis("off")
        # plt.title("相机分割")
        self.send_img("img_label_seg")

        # 画目标检测图
        # plt.subplot(2, 2, 2)
        plt.figure()
        plt.imshow(img_obj, cmap="gray" if "depth" in im_depth.name.lower() else None)
        plt.axis('off')
        # plt.title("目标检测")

        for key, value in object_pixels.items():
            if key == 0:
                continue
            if key not in objs_id.keys():
                continue
            if key in [91, 84, 96, 87, 102, 106, 120, 85, 113, 101, 83, 251]:
                X = np.array(value)
                db.fit(X)
                labels = db.labels_
                # 将数据按照聚类标签分组，并打印每个分组的数据
                for i in range(max(labels) + 1):  # 从0到最大聚类标签的值
                    group_data = X[labels == i]  # 获取当前标签的数据
                    x_max = max(p[0] for p in group_data)
                    y_max = max(p[1] for p in group_data)
                    x_min = min(p[0] for p in group_data)
                    y_min = min(p[1] for p in group_data)
                    if x_max - x_min < 10 or y_max - y_min < 10:
                        continue
                    if key != 251:
                        obj_detect_count += 1
                    else:
                        center_point = [int((x_max - x_min) / 2) + x_min, int((y_max-y_min) / 2) + y_min]
                        world_point = self.transform_co(img_data_depth, center_point[0],center_point[1] , d_depth[center_point[0]][center_point[1]][0], scene)
                        cur_obstacle_world_points.append([world_point[0], world_point[1]])
                        walker_detect_count += 1
                    # 在指定的位置绘制方框
                    # 创建矩形框
                    rect = patches.Rectangle((x_min, y_min), (x_max - x_min), (y_max - y_min), linewidth=1,
                                             edgecolor=self.colors[key % 10],
                                             facecolor='none')
                    plt.text(x_min, y_min, f'{objs_id[key]}',
                             fontdict={'family': 'serif', 'size': 10, 'color': 'green'}, ha='center',
                             va='center')
                    plt.gca().add_patch(rect)
            else:
                if key != 251:
                    obj_detect_count += 1
                else:
                    center_point = [int((x_max - x_min) / 2), int(y_max - y_min) / 2]
                    cur_obstacle_world_points.append(self.transform_co(img_data_depth, center_point[0], center_point[1],
                                                                       d_depth[center_point[0]][center_point[1]][0],
                                                                       scene))
                    walker_detect_count += 1
                x_max = max(p[0] for p in value)
                y_max = max(p[1] for p in value)
                x_min = min(p[0] for p in value)
                y_min = min(p[1] for p in value)
                # 在指定的位置绘制方框
                # 创建矩形框
                rect = patches.Rectangle((x_min, y_min), (x_max - x_min), (y_max - y_min), linewidth=1,
                                         edgecolor=self.colors[key % 10],
                                         facecolor='none')

                plt.text(x_min, y_min, f'{objs_id[key]}',
                         fontdict={'family': 'serif', 'size': 10, 'color': 'green'},
                         ha='center',
                         va='center')
                plt.gca().add_patch(rect)

        self.send_img("img_label_obj")

        if is_nav:
            self.navigator.planner.draw_graph(self.navigator.cur_step_num, self.navigator.yaw)
        else:
            new_map = self.updateMap(cur_obstacle_world_points)
            self.draw_map(plt,new_map)



        plt.axis("off")
        self.send_img("img_label_map")



        # plt.subplot(2, 7, 14)
        # plt.text(0, 0.9, f'检测行人数量：{walker_detect_count}', fontsize=10)
        # plt.text(0, 0.7, f'检测物体数量：{obj_detect_count}', fontsize=10)
        # plt.text(0, 0.5, f'新增语义信息：{walker_detect_count}', fontsize=10)
        # plt.text(0, 0.3, f'更新语义信息：{update_info_count}', fontsize=10)
        # plt.text(0, 0.1, f'已存语义信息：{self.infoCount}', fontsize=10)
        # 元组传参 pyqt5中的函数名， 四个参数

        # self.ui_func(("get_info", walker_detect_count, obj_detect_count, update_info_count, self.infoCount))
        semantic_info_str=""
        semantic_info_str+=  f'检测行人数量：{walker_detect_count}'+"\n\n"
        semantic_info_str += f'检测物体数量：{obj_detect_count}' + "\n\n"
        semantic_info_str += f'更新语义信息：{update_info_count}' + "\n\n"


        file_json_name = os.path.join(root_path, 'robowaiter/proto/objs.json')
        with open(file_json_name) as f:
            data = json.load(f)
        semantic_info_str += f'已存语义信息：{len(data)}' + "\n"

        # print("======semantic_info_str===========")

        self.ui_func(("get_semantic_info", semantic_info_str))

        # draw figures

        # output_path = os.path.join(self.output_path,"vision.png")




        # # 转换为numpy数组
        # image_np = np.asarray(image_pil)
        # self.ui_func(("draw_from_file","img_label_canvas",output_path))
        # plt.close()
        # plt.show()
        # return cur_obstacle_world_points

    def send_img(self,name):
        # 将图像保存到内存
        buffer = io.BytesIO()
        plt.savefig(buffer, bbox_inches='tight', pad_inches=0,format='png')
        image_data = buffer.getvalue()

        # 关闭当前图像
        plt.close()

        if not self.img_cache[name] == image_data:
            self.img_cache[name] = image_data
            self.ui_func(("draw_img",name,image_data))





    def updateMap(self, points):
        # map = copy.deepcopy(self.map_map)
        map = copy.deepcopy(self.map_map_real)
        for point in points:
            if point[0] < -350 or point[0] > 600 or point[1] < -400 or point[1] > 1450:
                continue
            map[math.floor((point[0] + 350) / self.map_ratio), math.floor((point[1] + 400) / self.map_ratio)] = 1
        for obs in points:
            obs = self.navigator.planner.real2map(obs)
            (x, y) = obs
            occupy_radius = self.navigator.planner.dyna_obs_radius  # 避免robot被dyna_obs的占用区域包裹住
            # 圆形区域
            occupy_pos = [(i, j) for i in range(x - occupy_radius, x + occupy_radius + 1)
                          for j in range(y - occupy_radius, y + occupy_radius + 1)
                          if euclidean_distance((i, j), obs) < occupy_radius]

            for pos in occupy_pos:
                map[pos] = 1
        return map

    def draw_map(self,plt, map):
        # plt.subplot(2, 1, 2)  # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
        plt.imshow(map, cmap='binary', alpha=0.5, origin='lower',
                   extent=(-400 / self.map_ratio, 1450 / self.map_ratio,
                           -350 / self.map_ratio, 600 / self.map_ratio))
        # plt.title('可达性地图')

    def get_id_object_world(self, id, scene):
        pixels = []
        world_points = []

        img_data_segment,img_data_depth,_ = self.get_cameras()
        # img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment])
        im_segment = img_data_segment.images[0]

        # img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth])
        im_depth = img_data_depth.images[0]

        d_segment = np.frombuffer(im_segment.data, dtype=im_segment.dtype).reshape(
            (im_segment.height, im_segment.width, im_segment.channels))
        d_depth = np.frombuffer(im_depth.data, dtype=im_depth.dtype).reshape(
            (im_depth.height, im_depth.width, im_depth.channels))

        d_segment = np.transpose(d_segment, (1, 0, 2))
        d_depth = np.transpose(d_depth, (1, 0, 2))

        for i in range(0, d_segment.shape[0], 5):
            for j in range(0, d_segment.shape[1], 5):
                if d_segment[i][j][0] == id:
                    pixels.append([i, j])
        for pixel in pixels:
            world_points.append(self.transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0]))
        return world_points

    def get_cameras(self):
        # if self.time - self.last_camera_time > self.camera_interval:
        self.img_data_segment = self.get_camera([GrabSim_pb2.CameraName.Head_Segment])
        time.sleep(0.2)
        self.img_data_depth = self.get_camera([GrabSim_pb2.CameraName.Head_Depth])
        time.sleep(0.2)
        self.img_data_color = self.get_camera([GrabSim_pb2.CameraName.Head_Color])
        # self.last_camera_time = self.time

        return self.img_data_segment,self.img_data_depth,self.img_data_color